/*
 * DriveSystem.cpp
 *
 *  Created on: Jan 5, 2011
 *      Author: Robotics
 */

#include "DriveSystem.h"
#include "GearBox.h"

DriveSystem::DriveSystem() {
	//

	mptr_leftGearbox = new GearBox(4,1, false);
	mptr_rightGearbox = new GearBox(4,3, true);
}

DriveSystem::~DriveSystem() {
	//
}
/*Function for autonomous that you can assign a number for the gearbox to drive. 
 * l would be how far you want the left gearbox to run.
 * r would be for right gearbox. 
 */
void DriveSystem::drive(float l, float r){
	mptr_leftGearbox->drive(l);
	mptr_rightGearbox->drive(r);
}

/*Driver control using the joysticks on the gamepad. This function uses the number
 * returned from the gamepad to drive the gearboxes.  There is also a button for
 * halfspeed. 
 */
void DriveSystem::drive(Gamepad* ptr_driverControl){
	
	//if button is pushed, go halfspeed
	if (ptr_driverControl->GetNumberedButton(6))
	{
		mptr_leftGearbox->drive(-ptr_driverControl->GetLeftY()/2);
		mptr_rightGearbox->drive(-ptr_driverControl->GetRightY()/2);
	}
	else 
	{
		mptr_leftGearbox->drive(-ptr_driverControl->GetLeftY());
		mptr_rightGearbox->drive(-ptr_driverControl->GetRightY());
			
	}
}
/*
void DriveSystem::drive(AutonomousSystem* ptr_autonMode){
	//
	//m_leftGearbox.drive(ptr_autonMode->);
	//m_rightGearbox.drive(ptr_autonMode->);
}*/

void DriveSystem::driveStraight(int dist){
	//mptr_leftGearbox->drive(dist);
	//mptr_rightGearbox->drive(dist);
}

void DriveSystem::turn(int theta){
	//mptr_leftGearbox->drive(theta);
	//mptr_rightGearbox->drive(theta);
}

/*Returns the encoder values for left and right so another file can use the numbers.
 * 
 * 
 */
double DriveSystem::getEncoderValue(bool leftEncoder){
	if (leftEncoder)
	{
		return mptr_leftGearbox->getEncoderValue();	
	}
	return mptr_rightGearbox->getEncoderValue();
}

GearBox* DriveSystem::getGearboxPtr(bool isRightGearbox){
	if(isRightGearbox){
		return mptr_rightGearbox;
	}
	
	else{
		return mptr_leftGearbox;
	}
}

//TODO: test acccuracy of average value
double DriveSystem::getAverageLREncoderValue(){
	return (mptr_leftGearbox->getEncoderValue() + mptr_rightGearbox->getEncoderValue())/2;
}




